import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster
from tf_transformations import quaternion_from_euler


class StaticTFBroadcaster(Node):
    def __init__(self):
        super().__init__('static_tf_broadcaster')
        self.static_broadcaster_ = StaticTransformBroadcaster(self)
        self.static_transforms=[]
        self.publish_static()

    def publish_static(self):
        t = TransformStamped()
        t.header.frame_id = 'map'
        t.header.stamp = self.get_clock().now().to_msg()
        t.child_frame_id = 'base_footprint'

        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0

        q = quaternion_from_euler(0,0,0,'rxyz')
        t.transform.rotation.x=q[0]
        t.transform.rotation.y=q[1]
        t.transform.rotation.z=q[2]
        t.transform.rotation.w=q[3]

        self.static_transforms.append(t)
    
        t1 = TransformStamped()
        t1.header.frame_id = 'base_footprint'
        t1.header.stamp = self.get_clock().now().to_msg()
        t1.child_frame_id = 'base_link'

        t1.transform.translation.x = 0.0
        t1.transform.translation.y = 0.0
        t1.transform.translation.z = 0.0

        q = quaternion_from_euler(0,0,0,'rxyz')
        t1.transform.rotation.x=q[0]
        t1.transform.rotation.y=q[1]
        t1.transform.rotation.z=q[2]
        t1.transform.rotation.w=q[3]

        self.static_transforms.append(t1)

        t2 = TransformStamped()
        t2.header.frame_id = 'base_link'
        t2.header.stamp = self.get_clock().now().to_msg()
        t2.child_frame_id = 'laser'
        t2.transform.translation.x = 0.0
        t2.transform.translation.y = 0.0
        t2.transform.translation.z = 0.3

        q = quaternion_from_euler(0,0,0,'rxyz')
        t2.transform.rotation.x=q[0]
        t2.transform.rotation.y=q[1]
        t2.transform.rotation.z=q[2]
        t2.transform.rotation.w=q[3]
        self.static_transforms.append(t2)


        # t3 = TransformStamped()
        # t3.header.frame_id = 'map'
        # t3.header.stamp = self.get_clock().now().to_msg()
        # t3.child_frame_id = 'odom'
        # t3.transform.translation.x = 0.0
        # t3.transform.translation.y = 0.0
        # t3.transform.translation.z = 0.0

        # q = quaternion_from_euler(0,0,0,'rxyz')
        # t3.transform.rotation.x=q[0]
        # t3.transform.rotation.y=q[1]
        # t3.transform.rotation.z=q[2]
        # t3.transform.rotation.w=q[3]
        # self.static_transforms.append(t3)
        self.static_broadcaster_.sendTransform(self.static_transforms)

def main(args = None):
    rclpy.init(args=args)
    node = StaticTFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()
